from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os
from launch.actions import Shutdown
from ament_index_python import get_package_share_directory

'''
ros2 launch stage_ros2 trobot_stage.launch.py
ros2 launch maps_manager test_maps_manager.launch.py
ros2 launch startup trobot_simu_cartographer.launch.py
ros2 param set /cartographer_node use_sim_time true


'''

def generate_launch_description():

    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    bringup_dir = get_package_share_directory('startup')
    global_parameter_file_arg = DeclareLaunchArgument(
        'global_parameter_file', default_value = os.path.join(bringup_dir, 'params', 'global_params.yaml'),
        description='')
    global_parameter_file = LaunchConfiguration('global_parameter_file')

    cartographer_control_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_control_node',
        parameters = [global_parameter_file,
                      {'use_sim_time': LaunchConfiguration('use_sim_time')}],
        # arguments = [
        #     '-configuration_directory', pkg_share + '/configuration_files',
        #     '-configuration_basename', 'm50.lua'],
        remappings = [
            ('imu', 'imu_raw'),
            # ('odom', 'car/odom''),
            ('scan', 'scan_raw'),
            ('points2', 'cloud_undistortion'),
            ],
        output = 'screen'
        )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': True},
            {'resolution': 0.05}],
        )
    
    rviz_node = Node(
        package = 'rviz2',
        executable = 'rviz2',
        on_exit = Shutdown(),
        arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/m50.rviz'],
        parameters = [{'use_sim_time': True}],
    )

    return LaunchDescription([
        use_sim_time_arg,
        global_parameter_file_arg,
        cartographer_control_node,
        cartographer_occupancy_grid_node,
        # rviz_node,
    ])

